/**
*	@class Matrix2
*
*	@brief
*
*	@author William McVicar
*/

#pragma once

#ifndef __MATRIX2_H_INCLUDED__
#define __MATRIX2_H_INCLUDED__

//  Library Includes

//  Local Includes
#include "Vector2.h"

//  Macros

//  Forward Declarations

//  Types

//  Constants

//  Prototypes

namespace Protein
{

template < class Real >
class Matrix2
{
	//Member Functions
public:

  Matrix2( bool _bZero = true );
  Matrix2( const Matrix2& other );
  Matrix2( Real a, Real b, Real c, Real d );
  Matrix2( const Real entry[4], bool rowMajor );
  Matrix2( const Vector2<Real>& _krU, const Vector2<Real>& v, bool _bColumns );
  Matrix2( const Vector2<Real>* _aV, bool _bColumns );
  Matrix2( Real a, Real d );
  Matrix2( Real angle );
  ~Matrix2();

  void makeZero();
  void makeIdentity();
  void makeDiagonal( Real a, Real d );
  void fromAngle( Real angle );

  inline              operator const Real*() const;
  inline              operator Real*();
  inline const Real*  operator[]( int row ) const;
  inline Real*        operator[]( int row );
  inline Real         operator()( int row, int col ) const;
  inline Real&        operator()( int row, int col );
  inline Matrix2&     operator=(const Matrix2& rhs);

  void SetRow( int row, const Vector2<Real>& v );
  Vector2<Real> GetRow( int row ) const;

  void SetColumn( int col, const Vector2<Real>& v );
  Vector2<Real> GetColumn( int col ) const;

  void GetColumnMajor( Real* _afColumnMajor ) const;

  bool operator==(const Matrix2& rhs) const;
  bool operator!=(const Matrix2& rhs) const;
  bool operator< (const Matrix2& rhs) const;
  bool operator<=(const Matrix2& rhs) const;
  bool operator> (const Matrix2& rhs) const;
  bool operator>=(const Matrix2& rhs) const;

  inline Matrix2 operator+(const Matrix2& rhs) const;
  inline Matrix2 operator-(const Matrix2& rhs) const;
  inline Matrix2 operator*(const Matrix2& rhs) const;
  inline Matrix2 operator*(Real rhs) const;
  inline Matrix2 operator/(Real rhs) const;
  inline Matrix2 operator-() const;

  inline Matrix2& operator+=(const Matrix2& rhs);
  inline Matrix2& operator-=(const Matrix2& rhs);
  inline Matrix2& operator*=(Real rhs);
  inline Matrix2& operator/=(Real rhs);

  inline Vector2<Real> operator*(const Vector2<Real>& v) const;

  Matrix2 Transpose() const;
  Matrix2 TransposeTimes( const Matrix2& rhs ) const;
  Matrix2 TimesTranspose( const Matrix2& rhs ) const;
  Matrix2 Inverse() const;
  Matrix2 Adjoint() const;
  Real Determinant() const;

  void ToAngle( Real& _rfAngle ) const;

protected: 

private: 

	int CompareArrays( const Matrix2& _krMat ) const;

	//Member Variables
public:

  static const Matrix2 Zero;
  static const Matrix2 Identity;

protected: 

private: 

	Real m_data[4];

};

template <class Real>
inline Matrix2<Real> operator*( Real _fScalar, const Matrix2<Real>& _krMatrix );

template <class Real>
inline Vector2<Real> operator*( const Vector2<Real>& vector, const Matrix2<Real>& _krMatrix);

template <class Real>
Matrix2<Real>::Matrix2( bool bZero )
{
  ( bZero ) ? MakeZero() : MakeIdentity();
}

template <class Real>
Matrix2<Real>::Matrix2(const Matrix2& rhs)
{
  memcpy( m_data, rhs.m_data, sizeof( Real ) * 4 ); 
}

template <class Real>
Matrix2<Real>::Matrix2( Real a, Real b, Real c, Real d )
{
  m_data[0] = a;
  m_data[1] = b;
  m_data[2] = c;
  m_data[3] = d;
}

template <class Real>
Matrix2<Real>::Matrix2(const Real entry[4], bool rowMajor)
{
  if(rowMajor)
  {
    memcpy( m_data, entry, sizeof( Real ) * 4 ); 
  }
  else
  {
    m_data[0] = entry[0];
    m_data[1] = entry[2];
    m_data[2] = entry[1];
    m_data[3] = entry[3];
  }
}

template <class Real>
Matrix2<Real>::Matrix2(const Vector2<Real>& u, const Vector2<Real>& v, bool _bColumns)
{
  if(_bColumns)
  {
    m_data[0] = u.x;
    m_data[1] = v.x;
    m_data[2] = u.y;
    m_data[3] = v.y;
  }
  else
  {
    m_data[0] = u.x;
    m_data[1] = u.y;
    m_data[2] = v.x;
    m_data[3] = v.y;
  }
}

template <class Real>
Matrix2<Real>::Matrix2(const Vector2<Real>* _aVectors, bool _bColumns)
{
  if(_bColumns)
  {
    m_data[0] = _aVectors[0][0];
    m_data[1] = _aVectors[1][0];
    m_data[2] = _aVectors[0][1];
    m_data[3] = _aVectors[1][1];
  }
  else
  {
    m_data[0] = _aVectors[0][0];
    m_data[1] = _aVectors[0][1];
    m_data[2] = _aVectors[1][0];
    m_data[3] = _aVectors[1][1];
  }
}

template <class Real>
Matrix2<Real>::Matrix2(Real a, Real d)
{
  MakeDiagonal(a,d);
}

template <class Real>
Matrix2<Real>::~Matrix2()
{
	//
}

template <class Real>
Matrix2<Real>::Matrix2(Real angle)
{
  FromAngle(angle);
}

template <class Real>
inline Matrix2<Real>::operator const Real*() const
{
  return m_data;
}

template <class Real>
inline Matrix2<Real>::operator Real*()
{
  return m_data;
}

template <class Real>
inline const Real* Matrix2<Real>::operator[](i32 _iRow) const
{
  return &m_data[2*_iRow];
}

template <class Real>
inline Real* Matrix2<Real>::operator[](i32 _iRow)
{
  return &m_data[2*_iRow];
}

template <class Real>
inline Real Matrix2<Real>::operator()(i32 _iRow, i32 _iCol) const
{
  return m_data[_iCol + 2*_iRow];
}

template <class Real>
inline Real& Matrix2<Real>::operator()(i32 _iRow, i32 _iCol)
{
  return m_data[_iCol + 2*_iRow];
}

template <class Real>
void Matrix2<Real>::makeZero()
{
  memset( m_data, Real(0), sizeof( Real ) ); 
}

template <class Real>
void Matrix2<Real>::makeIdentity()
{
  m_data[0] = Real(1.0);
  m_data[1] = Real(0.0);
  m_data[2] = Real(0.0);
  m_data[3] = Real(1.0);
}

template <class Real>
void Matrix2<Real>::makeDiagonal(Real a, Real d)
{
  m_data[0] = a;
  m_data[1] = Real(0.0);
  m_data[2] = Real(0.0);
  m_data[3] = d;
}

template <class Real>
void Matrix2<Real>::fromAngle(Real angle)
{
  m_data[0] =  Math<Real>::Cos(angle);
  m_data[2] =  Math<Real>::Sin(angle);
  m_data[1] = -m_data[2];
  m_data[3] =  m_data[0];
}

template <class Real>
void Matrix2<Real>::SetRow(i32 _iRow, const Vector2<Real>& v)
{
  m_data[ 2*_iRow]    = v.m_afComponents[0];
  m_data[(2*_iRow)+1] = v.m_afComponents[1];
}

template <class Real>
Vector2<Real> Matrix2<Real>::GetRow(i32 _iRow) const
{
  return Vector2<Real>(m_data[2*_iRow],m_data[(2*_iRow)+1]);
}

template <class Real>
void Matrix2<Real>::SetColumn(i32 _iCol, const Vector2<Real>& v)
{
  m_data[_iCol]		= v.x;
  m_data[_iCol+2]	= v.y;
}

template <class Real>
Vector2<Real> Matrix2<Real>::GetColumn(i32 _iCol) const
{
  return Vector2<Real>(m_data[_iCol],m_data[_iCol+2]);
}

template <class Real>
void Matrix2<Real>::GetColumnMajor(Real* columnMajor) const
{
  columnMajor[0] = m_data[0];
  columnMajor[1] = m_data[2];
  columnMajor[2] = m_data[1];
  columnMajor[3] = m_data[3];
}

template <class Real>
inline Matrix2<Real>& Matrix2<Real>::operator=( const Matrix2& rhs )
{
  memcpy( m_data, rhs.m_data, sizeof( Real ) * 4 );
  return *this;
}

template <class Real>
i32 Matrix2<Real>::CompareArrays(const Matrix2& rhs) const
{
  return memcmp(m_data,rhs.m_data,4*sizeof(Real));
}

template <class Real>
bool Matrix2<Real>::operator==(const Matrix2& rhs) const
{
  return CompareArrays(rhs) == 0;
}

template <class Real>
bool Matrix2<Real>::operator!=(const Matrix2& rhs) const
{
  return CompareArrays(rhs) != 0;
}

template <class Real>
bool Matrix2<Real>::operator< (const Matrix2& rhs) const
{
  return CompareArrays(rhs) < 0;
}

template <class Real>
bool Matrix2<Real>::operator<=(const Matrix2& rhs) const
{
  return CompareArrays(rhs) <= 0;
}

template <class Real>
bool Matrix2<Real>::operator> (const Matrix2& rhs) const
{
  return CompareArrays(rhs) > 0;
}

template <class Real>
bool Matrix2<Real>::operator>=(const Matrix2& rhs) const
{
  return CompareArrays(rhs) >= 0;
}

template <class Real>
inline Matrix2<Real> Matrix2<Real>::operator+(const Matrix2& rhs) const
{
  return Matrix2<Real>( m_data[0] + rhs.m_data[0],
                        m_data[1] + rhs.m_data[1],
                        m_data[2] + rhs.m_data[2],
                        m_data[3] + rhs.m_data[3]);
}

template <class Real>
inline Matrix2<Real> Matrix2<Real>::operator-(const Matrix2& rhs) const
{
  return Matrix2<Real>( m_data[0] - rhs.m_data[0],
                        m_data[1] - rhs.m_data[1],
                        m_data[2] - rhs.m_data[2],
                        m_data[3] - rhs.m_data[3]);
}

template <class Real>
inline Matrix2<Real> Matrix2<Real>::operator*(const Matrix2& rhs) const
{
  return Matrix2<Real>( m_data[0]*rhs.m_data[0] + m_data[1]*rhs.m_data[2],
                        m_data[0]*rhs.m_data[1] + m_data[1]*rhs.m_data[3],
                        m_data[2]*rhs.m_data[0] + m_data[3]*rhs.m_data[2],
                        m_data[2]*rhs.m_data[1] + m_data[3]*rhs.m_data[3]);
}

template <class Real>
inline Matrix2<Real> Matrix2<Real>::operator*(Real rhs) const
{
  return Matrix2<Real>( rhs*m_data[0], rhs*m_data[1],
						            rhs*m_data[2], rhs*m_data[3]);
}

template <class Real>
inline Matrix2<Real> Matrix2<Real>::operator/(Real rhs) const
{
  if(rhs != Real(0))
  {
    Real fInvScalar =(Real(1.0))/rhs;
    return Matrix2<Real>( fInvScalar*m_data[0], fInvScalar*m_data[1],
							            fInvScalar*m_data[2], fInvScalar*m_data[3]);
  }

  return Matrix2<Real>( Math<Real>::MAX_REAL, Math<Real>::MAX_REAL,
						            Math<Real>::MAX_REAL, Math<Real>::MAX_REAL );
}

template <class Real>
inline Matrix2<Real> Matrix2<Real>::operator-() const
{
    return Matrix2<Real>( -m_data[0], -m_data[1],
						              -m_data[2], -m_data[3] );
}

template <class Real>
inline Matrix2<Real>& Matrix2<Real>::operator+=(const Matrix2& rhs)
{
    m_data[0] += rhs.m_data[0];
    m_data[1] += rhs.m_data[1];
    m_data[2] += rhs.m_data[2];
    m_data[3] += rhs.m_data[3];
    return *this;
}

template <class Real>
inline Matrix2<Real>& Matrix2<Real>::operator-=(const Matrix2& rhs)
{
    m_data[0] -= rhs.m_data[0];
    m_data[1] -= rhs.m_data[1];
    m_data[2] -= rhs.m_data[2];
    m_data[3] -= rhs.m_data[3];
    return *this;
}

template <class Real>
inline Matrix2<Real>& Matrix2<Real>::operator*=(Real rhs)
{
    m_data[0] *= rhs;
    m_data[1] *= rhs;
    m_data[2] *= rhs;
    m_data[3] *= rhs;
    return *this;
}

template <class Real>
inline Matrix2<Real>& Matrix2<Real>::operator/=(Real rhs)
{
  if(rhs !=Real(0))
  {
      Real fInvScalar =(Real(1.0))/rhs;
      m_data[0] *= fInvScalar;
      m_data[1] *= fInvScalar;
      m_data[2] *= fInvScalar;
      m_data[3] *= fInvScalar;
  }
  else
  {
      m_data[0] = Math<Real>::MAX_REAL;
      m_data[1] = Math<Real>::MAX_REAL;
      m_data[2] = Math<Real>::MAX_REAL;
      m_data[3] = Math<Real>::MAX_REAL;
  }

  return *this;
}

template <class Real>
inline Vector2<Real> Matrix2<Real>::operator*(const Vector2<Real>& v) const
{
  return Vector2<Real>( m_data[0]*v.x + m_data[1]*v.y,
					              m_data[2]*v.x + m_data[3]*v.y );
}

template <class Real>
Matrix2<Real> Matrix2<Real>::Transpose() const
{
  return Matrix2<Real>( m_data[0], m_data[2],
						            m_data[1], m_data[3]);
}

template <class Real>
Matrix2<Real> Matrix2<Real>::TransposeTimes(const Matrix2& rhs) const
{
  // P = A^T*B
  return Matrix2<Real>( m_data[0]*rhs.m_data[0] + m_data[2]*rhs.m_data[2],
						            m_data[0]*rhs.m_data[1] + m_data[2]*rhs.m_data[3],
						            m_data[1]*rhs.m_data[0] + m_data[3]*rhs.m_data[2],
						            m_data[1]*rhs.m_data[1] + m_data[3]*rhs.m_data[3] );
}

template <class Real>
Matrix2<Real> Matrix2<Real>::TimesTranspose(const Matrix2& rhs) const
{
  // P = A*B^T
  return Matrix2<Real>( m_data[0]*rhs.m_data[0] + m_data[1]*rhs.m_data[1],
						            m_data[0]*rhs.m_data[2] + m_data[1]*rhs.m_data[3],
						            m_data[2]*rhs.m_data[0] + m_data[3]*rhs.m_data[1],
						            m_data[2]*rhs.m_data[2] + m_data[3]*rhs.m_data[3]  );
}

template <class Real>
Matrix2<Real> Matrix2<Real>::Inverse() const
{
  Matrix2<Real> Inverse;

  Real fDet = m_data[0]*m_data[3] - m_data[1]*m_data[2];
  if(Math<Real>::FAbs(fDet) > Math<Real>::ZERO_TOLERANCE)
  {
    Real fInvDet	     =  (Real(1.0))/fDet;
    Inverse.m_data[0] =  m_data[3]*fInvDet;
    Inverse.m_data[1] = -m_data[1]*fInvDet;
    Inverse.m_data[2] = -m_data[2]*fInvDet;
    Inverse.m_data[3] =  m_data[0]*fInvDet;
  }
  else
  {
    Inverse.m_data[0] =Real(0.0)
    Inverse.m_data[1] =Real(0.0)
    Inverse.m_data[2] =Real(0.0)
    Inverse.m_data[3] =Real(0.0)
  }

  return Inverse;
}

template <class Real>
Matrix2<Real> Matrix2<Real>::Adjoint() const
{
  return Matrix2<Real>( m_data[3], -m_data[1],
						           -m_data[2],  m_data[0]);
}

template <class Real>
Real Matrix2<Real>::Determinant() const
{
  return m_data[0]*m_data[3] - m_data[1]*m_data[2];
}

template <class Real>
void Matrix2<Real>::ToAngle(Real& _rfAngle) const
{
  _rfAngle = Math<Real>::ATan2(m_data[2],m_data[0]);
}

template <class Real>
inline Matrix2<Real> operator*(Real scalar, const Matrix2<Real>& rhs)
{
  return rhs*scalar;
}

template <class Real>
inline Vector2<Real> operator*(const Vector2<Real>& v, const Matrix2<Real>& rhs)
{
  return Vector2<Real>( v.x*rhs[0][0] + v.y*rhs[1][0],
                        v.x*rhs[0][1] + v.y*rhs[1][1] );
}

typedef Matrix2<float>  Matrix2f;
typedef Matrix2<double> Matrix2d;

}

#endif //__MATRIX2_H_INCLUDED__
